#pragma once

#include <pthread.h>
#include <list>
using namespace std;
#include <mutex>
#include <thread>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <arpa/inet.h>

#include "rclcpp/rclcpp.hpp"

#include <agv_srvs/srv/version.hpp>
#include <std_msgs/msg/string.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/bat_info.hpp>
#include <agv_msgs/msg/agv_mode.hpp> // 2024.1.15
#include <agv_msgs/msg/state.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/inputstatus.hpp>
#include <agv_msgs/msg/inputdevices.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/out_puts_new.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <agv_msgs/msg/set_stop_bits.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/tape_sensors.hpp>
#include <agv_msgs/msg/nav_type.hpp>
#include <agv_msgs/msg/connection_monitor.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <geometry_msgs/msg/twist_stamped.hpp>
#include "toolKit.h"

#define FORKLIFT_JOINT "forkLift_joint"
#define BUMPERSHIELD_STATE "bumperShieldFeedback"
#define BUMPERSHIELD_RELAY "bumperShield"
#define TIMEOUT_S (0.5)

struct Scene;

enum SceneIndex
{
	IDLE = 0,		//1:车辆空闲
	DRIVING,		//2:正常，移动中
	MANUAL,			//3:车辆手动状态
	TURN_LEFT,		//4:左拐弯
	TURN_RIGHT,		//5:右拐弯
	ROTATION,		//6:自旋
	BACKWARD,		//7:后退
	SEMI_AUTO,		//8:车辆在线半自动状态
	CHARGING,		//9:正在充电
	TEMP_PARKING,	//10:车辆临时停车
	WORKING,    	//11:工装平台正在动作
	QR_FOUND,   	//12:手动模式下，找到地面二维码
	TAPE_FOUND2,   	//13:手动模式下，找到两条磁条
	TRAFFIC_RESTRICT, //14:车辆管制
	PAUSED,    		//15:任务暂停
	LOW_BATTERY,   	//16:低电量预警
	OFFLINE,        //17:未上线
	DANGER_AREA,   	//18:车辆到达特殊工位     
	WAITING_PERMISSION,   	//19:等待外部信号中      
	SAFETY_SCANNER_SLOWDOWN,//20:车辆避障提醒、预警、减速（后退）
	START_DELAY,   //21:车辆启动中（启动前提醒）
	LOST_HOST,      //22:与主控失去连接
	TOO_LOW_BATTERY, 	//23:电量过低 
	OFF_PATH,      //24:AGV脱离路线
	TASK_CANCEL,      //25 AGV任务取消
	LOADER_ACTION_FAILED, //26车辆异常停止（动作异常）
	VEHICLE_SAFETY_ACK,   //27车辆异常停止（安全确认）
	NAVIGATION_FAILED,    //28激光导航故障/导航失败/激光定位错误
	SAFETY_SENSOR_BYPASS, //29安全旁路屏蔽提示
	VEHICLE_FAULT,        //30车辆异常停止（硬件、软件故障）
	SAFETY_SCANNER_STOP,  //31避障保护区域/车身光电避障
	COLLISION_SENSOR,     //32安全急停中（触边）
	ESTOP_BUTTON          //32安全急停中（急停按钮）
};

struct EventTrig
{
	bool temp_parking;
	bool low_battery;  
	bool safety_scanner_slowDown;  
	bool lost_host;  
	bool too_low_battery;
	bool off_path;  
	bool task_cancel;  
	bool loader_action_failed;  
	bool vehicle_safety_ack;  
	bool location;  
	bool vehicle_fault;  
	bool safety_scanner_stop;  
	bool collision_sensor;  
	bool estop_button;
	
	EventTrig():temp_parking(false), 
				low_battery(false), 
				safety_scanner_slowDown(false), 
				lost_host(false), 
				too_low_battery(false),
				off_path(false), 
				task_cancel(false), 
				loader_action_failed(false), 
				vehicle_safety_ack(false), 
				location(false), 
				vehicle_fault(false), 
				safety_scanner_stop(false), 
				collision_sensor(false), 
				estop_button(false)
	{
	}
	void reset(void)
	{
		temp_parking= false;  
		low_battery= false; 
		safety_scanner_slowDown= false; 
		lost_host= false; 
		too_low_battery= false;
		off_path= false; 
		task_cancel= false; 
		loader_action_failed= false; 
		vehicle_safety_ack= false; 
		location= false; 
		vehicle_fault= false; 
		safety_scanner_stop= false; 
		collision_sensor= false; 
		estop_button= false;
	}
};
class VehicleStatus
{
public:
	VehicleStatus():protection_bypass_(false),
					protection_bypassed_(false),
					qrSensorDetect_(false)
	{
		
	};
	VehicleStatus(std::vector<Scene>* Ptr):	protection_bypass_(false),
											protection_bypassed_(false)
	{
		scenesPtr_= Ptr;
	};
	
	~VehicleStatus();
	int init(const std::shared_ptr<rclcpp::Node> node);

private:		
	bool spinRun();
	bool backward();
	bool navRun();
  void Batterys_Callback(const agv_msgs::msg::BatInfo::ConstSharedPtr  bat_msg);
	void agvmodeCallback(const agv_msgs::msg::AGVMode::ConstSharedPtr state_p);
	void AutoStatus_Callback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr  autoStatus_msg);
	void event_Callback(const agv_msgs::msg::AGVEventStatus::ConstSharedPtr &event);
	void setStopCallback(const agv_msgs::msg::SetStopBits::ConstSharedPtr &msg);
	void readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg);
	void readOutputCallback(const agv_msgs::msg::OutPutsNew::ConstSharedPtr &msg);	
	void landmarkCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg);
	void tapeSensor_Callback(const agv_msgs::msg::TapeSensors::ConstSharedPtr pMsg);
	void networkCB(const agv_msgs::msg::ConnectionMonitor::ConstSharedPtr &msg);
	void vda5050Callback(const agv_msgs::msg::State::ConstSharedPtr &msg);
	void poseCB(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg);
	void naviTypeCB(const agv_msgs::msg::NavType::ConstSharedPtr &msg);
	void eventCheck(int ecode, EventTrig &trigs);	
	void eventPolling(void);
	void eventPub(int index, agv_msgs::msg::AGVEvent *event);
	void cmd_velCallBack(const geometry_msgs::msg::TwistStamped::ConstSharedPtr &msg);
	void protection_bypassCheck(void);
	void eventSort2Scene(void);	
	bool qrCodeValidCheck(void);
	bool tapeValidCheck(void);
	void workingCheck(void);
	void runstateCheck(void);
	static void poll_looper(void *arg);
public:
	EventTrig getTrigs(void);
	bool poseCheck(void);
	bool networkCheck(void);
	bool alarmCheck(void);	
private:	
    enum Type
    {
      JACKING,
      ROTARY,
      JACKING_ROTARY,
      ROLLER,
      FORKLIFT
    } ;
  std::shared_ptr<rclcpp::Node> node_;
	std::thread tid_;
	std::timed_mutex quit_mutex_;
	rclcpp::Subscription<agv_msgs::msg::BatInfo>::SharedPtr battery_sub_;
	rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agvmode_sub_;
	rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr	autoStatus_sub_;
	rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr location_sub_;	
	rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr eventOccurSub_;
	// rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr	avoidSensorSub_;
	// rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr	ForkLiftSub_;
	// rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr	periph_status_Sub_;
	rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr	readinput_sub_;
	rclcpp::Subscription<agv_msgs::msg::SetStopBits>::SharedPtr	set_stop_sub_;
	rclcpp::Subscription<agv_msgs::msg::OutPutsNew>::SharedPtr	readoutput_sub_;
	rclcpp::Subscription<agv_msgs::msg::TapeSensors>::SharedPtr	tapeSensorSub_;
	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr	landmarkSub_;
	rclcpp::Subscription<agv_msgs::msg::ConnectionMonitor>::SharedPtr	network_connection_sub_;
	rclcpp::Subscription<agv_msgs::msg::NavType>::SharedPtr	navi_type_sub_;
	rclcpp::Subscription<agv_msgs::msg::State>::SharedPtr	vda5050_sub_;
	rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr odom_sub_;
	std::vector<Scene>* scenesPtr_;  
	EventTrig trigs_;
	agv_msgs::msg::AGVEventStatus agv_event_;
	agv_msgs::msg::AgvAutoStatus autoStatus_;
	agv_msgs::msg::State vda_;
	agv_msgs::msg::AGVMode mode_;
	agv_msgs::msg::ConnectionMonitor netMsg_;
	agv_msgs::msg::NavType naviType_;
	agv_msgs::msg::PoseWithConfidence poseMsg_;
	CDataTimeStamp<agv_msgs::msg::TapeSensors> tape_;	//磁条超时	
	CDataTimeStamp<bool> qrSensorDetect_;		//二维码超时
	bool 	protection_bypass_,
			protection_bypassed_,
			useBackAlarm_,
			useSpinAlarm_;
	int danger_bitIndex_;
	std::mutex 	mutex_,
				vdamutex_;				
};

